/**
 ******************************************************************************
 * @FilePath: \Hero\App\Motor_control.c
 * @Brief: 
 * @Date: 2021-01-11 23:21:09
 * @Author: Rio
 ******************************************************************************
 */
#include "Motor_control.h"
#include "bsp_can.h"
#include "string.h"

/*********************************** Private defines *****************************************/
#define USE_POSITION_ANGLE                        1//将电机码盘值转换为角度

/**********************************Interface function****************************************/
/**
 ******************************************************************************
 * @brief set basic paramter of PID
 * @param {PID_AbsoluteType} *pid
 * @param {float} p
 * @param {float} i
 * @param {float} d
 * @param {float} erriLim
 * @param {float} MaxOutCur
 * @retval 
 ******************************************************************************
 */
void PID_AbsParam_Init(PID_AbsoluteType *pid, float p, float i, float d, float erriLim, int16_t MaxOutCur)
{
	memset(pid, 0, sizeof(PID_AbsoluteType));

	pid->kp = p;
	pid->ki = i;
	pid->kd = d;

	pid->errNow = 0;
	pid->errLast = 0;
	pid->ctrOut = 0;
	pid->errILim = erriLim;
	pid->OutMAX = MaxOutCur;
}

void PID_AbsParam_Set(PID_AbsoluteType *pid, float p, float i, float d, float erriLim, int16_t MaxOutCur)
{
	pid->kp = p;
	pid->ki = i;
	pid->kd = d;
	
	pid->errILim = erriLim;
	pid->OutMAX = MaxOutCur;
}

/**
 ******************************************************************************
 * @brief use speed_loop_pid to control motor tending to target speed
 * @param {PID_AbsoluteType} pid
 * @param {s_motor_data_t} motor
 * @param {float} target
 * @retval output currency
 ******************************************************************************
 */
int16_t motor_speed_loop_pid_control(PID_AbsoluteType *spd_pid, float get, float target)
{
	static float output;
	spd_pid->errNow = (float)(target - get);
	PID_AbsoluteMode(spd_pid);
	output = (int16_t)spd_pid->ctrOut;

	return output;
}

/**
 ******************************************************************************
 * @brief use cascade_loop_pid to control motor tending to target position
 * @param {PID_AbsoluteType} spd_pid
 * @param {PID_AbsoluteType} pos_pid
 * @param {motorData_t} motor
 * @param {double} target
 * @retval output currency
 ******************************************************************************
 */
float motor_cascade_loop_pid_control(PID_AbsoluteType *pos_pid, PID_AbsoluteType *spd_pid, float externGet, float externSet, float internGet)
{
	static float output;
	static float out_st;

	pos_pid->errNow = (float)externSet - (float)externGet;
	PID_AbsoluteMode(pos_pid);
	out_st = pos_pid->ctrOut;

	spd_pid->errNow = out_st - (float)internGet;
	PID_AbsoluteMode(spd_pid);
	output = (int16_t)spd_pid->ctrOut;

	return output;
}

void process_motor_encoder_to_serial(motorData_t *motor)
{
	//处理成连续的码盘值
	if(motor->esc_back_position - motor->esc_back_position_last > 6000)
		--motor->circle_num;
	else if(motor->esc_back_position - motor->esc_back_position_last < -6000)
		++motor->circle_num;
	motor->serial_position = motor->esc_back_position + motor->circle_num * 8191.0f;

	if(USE_POSITION_ANGLE)
    {
		//码盘值转换成角度
	    motor->position_angle = motor->serial_position / 8191.0f * 360.0f;
	}
	motor->esc_back_position_last =  motor->esc_back_position;
}

/**
  * @brief  Send function of CAN1 by using the can transmit_IT to chassis
  * @param  hcan: CAN into interruption
  * 		current1: 1 to 2 byte of send data ID 为 0x201 的电机电流控制值，范围[-16384,16384]
  * 		current2: 3 to 4 byte of send data ID 为 0x202 的电机电流控制值，范围[-16384,16384]
  * 		current3: 5 to 6 byte of send data ID 为 0x203 的电机电流控制值，范围[-16384,16384]
  * 		current4: 7 to 8 byte of send data ID 为 0x204 的电机电流控制值，范围[-16384,16384]
  * @retval void
  * @usage  you can choose the suitable one to use
  *         according to the data type you want to transmit
  */
void CANTx_SendCurrent(CAN_HandleTypeDef *hcan,uint32_t id,int16_t current1,int16_t current2,int16_t current3,int16_t current4)
{
  CAN_Message tx_message;
	CAN_TxHeaderTypeDef CanTxHeader;

	CanTxHeader.StdId = id;
	CanTxHeader.IDE = CAN_ID_STD;
	CanTxHeader.RTR = CAN_RTR_DATA;
	CanTxHeader.DLC = 0x08;

	tx_message.Data[0]=(unsigned char)(current1>>8);
	tx_message.Data[1]=(unsigned char)current1;
	tx_message.Data[2]=(unsigned char)(current2>>8);
	tx_message.Data[3]=(unsigned char)current2;
	tx_message.Data[4]=(unsigned char)(current3>>8);
	tx_message.Data[5]=(unsigned char)current3;
	tx_message.Data[6]=(unsigned char)(current4>>8);
	tx_message.Data[7]=(unsigned char)current4;

	HAL_CAN_AddTxMessage(hcan,&CanTxHeader,tx_message.Data,(uint32_t*)CAN_TX_MAILBOX0);
}